#ifndef _TURN_H_
#define _TURN_H_

/**
 * "turnWithShaft" turns the robot to the given degree.
 *
 * The turn will depend on a mix of error detections from gyro and shaft 
 * encoders from both wheels
 *
 * @requires gyro must have been calibrated
 * @effects the robot is turned to the given degree
 * @modifies none
 */
void turnWithShaft();

/**
 * "turnEncoderErr" returns the error caused by difference in rates of turns by both wheels
 *
 * The error is calculated based on the difference of shafts detected by the left and the
 * right wheel of the robot
 *
 * @requires none
 * @effects returns the error value based on the shafts detected by both shaft encoders
 * @modifies none
 */
double turnEncoderErr();

/**
 * "turnGyroErr" returns the error caused by difference in expected shafts and shafts actually
 * detected up to the point. The expected value is based on the angle measured by the gyro
 *
 * The error is measured on the encoder with the given port
 *
 * @requires gyro must be calibrated before hand
 * @effects returns the error value based on the shafts detected on the encoder of the given port
 * @modifies none
 */
double turnGyroErr(int16_t startAngle, uint8_t encoderPort);

/*
 * "turn" turns the robot to the specified angle.
 * Angles are positive when turned clockwise and negative when turned counter-clockwise.
 * The angle is 0 when the direction is right in front of the robot.
 * 
 * @requires none
 * @effects the robot is turned to the specified angle.
 * @modifies none
 */
//void turn(int16_t deg);
/*
 * "turnLeft" is a more user-friendly version of "turn" where the degree put into the function is
 * more positive if turned counter-clockwise
 *
 * @requires degree >=0
 * @effects the robot is turned to the specified angle to the left
 * @modifies none
 */
void turnLeft(int16_t degree);

/*
 * "turnRight is a more user-friendly version of "turn" where the degree put into the function is
 * more positive if turned clockwise
 * 
 * It works exactly the same as "turn"
 *
 * @requires degree >=0
 * @effects the robot is turned to the specified angle to the right
 * @modifies none
 */

void turnRight(int16_t degree);

/**
 * turnToOrient turns the robot to the given orientation
 *
 * the currentSide will be updated to the default value, meaning that it
 * is the expected value for normal situation. The currentSide of value 4
 * will be manually adjusted in accident-handler functions.
 * 
 * @requires the robot is not obstructed in turning
 * @effects turns the robot to the given orientation
 * @modifies currentOrient, currentSide
 */
 void turnToOrient(int8_t orientNum);
 
/**
 * "calibrateGyro" lets the robot rest for a brief period of time and reset its angle back to zero
 *
 * @requires none
 * @effects the angle measured by gyroscope is reset to zero
 * @modifies the angle stored in gyroscope
 */
void calibrateGyro(void);

/**
 * "getCurrentAngle" returns the angle that the robot has turned during the turn
 * 
 * @requires none
 * @effects returns the angle that the robot has turned during the turn
 * @modifies none
 */
int16_t getCurrentAngle(void);

#endif
